package tests;

import static org.junit.Assert.assertTrue;
import static org.junit.Assert.fail;

import java.awt.geom.Point2D;
import org.junit.Test;
import base.Geometry;


public class GeometryTest {

	static final double errorLimit = 1E-12;
	
	@Test
	public void testGetX() {
		assertTrue(Math.abs(Geometry.getX(0) - 0) < errorLimit);
		
		assertTrue(Geometry.getX(90) == 1);
		assertTrue(Math.abs(Geometry.getX(180) - 0) < errorLimit);
		assertTrue(Geometry.getX(270) == -1);
		assertTrue(Math.abs(Geometry.getX(360) -0) < errorLimit);
		
		assertTrue(Math.abs(Geometry.getX(90+60) - 0.5) < errorLimit);
	}

	@Test
	public void testGetY() {
		assertTrue(Geometry.getY(0) == 1);
		assertTrue(Math.abs(Geometry.getY(90) - 0) < errorLimit);
		assertTrue(Math.abs(Geometry.getY(270) - 0) < errorLimit);
		assertTrue(Geometry.getY(180) == -1);
		
		assertTrue(Math.abs(Geometry.getY(90+30) + 0.5) < errorLimit);
	}

	@Test
	public void testMathToCompassDeg() {
		assertTrue(Geometry.mathToCompassDeg(90) == 0);
		assertTrue(Geometry.mathToCompassDeg(0) == 90);
		assertTrue(Geometry.mathToCompassDeg(180) == 270);
		assertTrue(Geometry.mathToCompassDeg(270) == 180);
		
		assertTrue(Geometry.mathToCompassDeg(360-30) == 90+30);
		assertTrue(Geometry.mathToCompassDeg(300) == 90+60);
	}

	@Test
	public void testGetHeading() {
		// these tests are for normal "mathematical" space
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 0, 1) -  90) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1, 1) -  45) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1, 0) -   0) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1,-1) - (270+45)) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 0,-1) - 270) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0,-1,-1) - (180+45)) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0,-1, 0) - 180) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0,-1, 1) - 135) < errorLimit );
		
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 100, 1)   -   1) < 1 );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 100, -1)  -   359) < 1 );
		
		assertTrue(Math.abs(Geometry.getHeading(0, 0, -100, -1) -  181) < 1 );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, -100, 1)  -  179) < 1 );
		
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1, 100)   -   90) < 1 );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, -1, 100)  -   90) < 1 );
		
		
		// these tests are for compass headings
		/*
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 0, 1) -   0) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1, 1) -  45) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1, 0) -  90) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 1,-1) - 135) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0, 0,-1) - 180) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0,-1,-1) - 225) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0,-1, 0) - 270) < errorLimit );
		assertTrue(Math.abs(Geometry.getHeading(0, 0,-1, 1) - 315) < errorLimit );
		
		assertTrue(Geometry.getHeading(0, 0, 100, 1)   -   89 < 1 );
		assertTrue(Geometry.getHeading(0, 0, 100, -1)  -   90 < 1 );
		
		assertTrue(Geometry.getHeading(0, 0, -100, -1) -  269 < 1 );
		assertTrue(Geometry.getHeading(0, 0, -100, 1)  -  270 < 1 );
		
		assertTrue(Geometry.getHeading(0, 0, 1, 100)   -    0 < 1 );
		assertTrue(Geometry.getHeading(0, 0, -1, 100)  -  359 < 1 );
		*/
	}

	@Test
	public void testMirrorHorizontally() {
		assertTrue(Geometry.mirrorHorizontally(0) == 0);
		assertTrue(Geometry.mirrorHorizontally(360) == 0);
		assertTrue(Geometry.mirrorHorizontally(-1) == 1);
		assertTrue(Geometry.mirrorHorizontally(1) == 359);
		
		assertTrue(Geometry.mirrorHorizontally(60) == 300);
		assertTrue(Geometry.mirrorHorizontally(300) == 60);
		
		assertTrue(Geometry.mirrorHorizontally(120) == 240);
		assertTrue(Geometry.mirrorHorizontally(240) == 120);
		
		assertTrue(Geometry.mirrorHorizontally(90) == 270);
		assertTrue(Geometry.mirrorHorizontally(270) == 90);
		
		assertTrue(Geometry.mirrorHorizontally(225) == 135);
		assertTrue(Geometry.mirrorHorizontally(215) == 145);
	}
	
	@Test
	public void testCompassToMathDeg () {
		assertTrue(Geometry.compassToMathDeg(0) == 90);
		assertTrue(Geometry.compassToMathDeg(90) == 0);
		assertTrue(Geometry.compassToMathDeg(270) == 180);
		assertTrue(Geometry.compassToMathDeg(180) == 270);
		
		assertTrue(Geometry.compassToMathDeg(120) == 330);
		assertTrue(Geometry.compassToMathDeg(150) == 300);
		
		//assertTrue()
		assertTrue(Geometry.compassToMathDeg(224) == 136);
		assertTrue(Geometry.compassToMathDeg(225) == 135);
	}
	
	@Test
	public void testTwoWays()
	{
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(0)) -0) < errorLimit);
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(12)) -12) < errorLimit);
		
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(112)) - 112) < errorLimit);
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(180)) - 180) < errorLimit);
		
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(265)) - 265) < errorLimit);
		
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(190)) - 190) < errorLimit);
		
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(311)) - 311) < errorLimit);
		
		assertTrue( Math.abs(Geometry.compassToMathDeg(Geometry.mathToCompassDeg(225)) - 225) < errorLimit);
	}
	
	@Test
	public void testSolveYforLine()
	{
		fail("Not yet implemented"); // TODO
	}
	
	public Point2D.Double [] vectorsFromRect(Point2D.Double[] corners)
	{
		Point2D.Double [] result = new Point2D.Double [4];
		result[0] = new Point2D.Double(corners[1].x - corners[0].x, corners[1].y - corners[0].y);
		result[1] = new Point2D.Double(corners[2].x - corners[1].x, corners[2].y - corners[1].y);
		result[2] = new Point2D.Double(corners[3].x - corners[2].x, corners[3].y - corners[2].y);
		result[3] = new Point2D.Double(corners[0].x - corners[3].x, corners[0].y - corners[3].y);
		return result;
	}
	
	public void orthogonal(Point2D.Double a, Point2D.Double b)
	{
		assertTrue(a.x * b.x + a.y * b.y < errorLimit);
	}
	
	@Test
	public void testRotateRect()
	{
		Point2D.Double [] corners = Geometry.rotateRect(15, 40, 10);
		Point2D.Double [] v = vectorsFromRect(corners);
		
		orthogonal(v[0], v[1]);
		orthogonal(v[1], v[2]);
		orthogonal(v[2], v[3]);
		orthogonal(v[3], v[0]);
		
		corners = Geometry.rotateRect(25, 40, 110);
		v = vectorsFromRect(corners);
		
		orthogonal(v[0], v[1]);
		orthogonal(v[1], v[2]);
		orthogonal(v[2], v[3]);
		orthogonal(v[3], v[0]);
		
		corners = Geometry.rotateRect(1, 1, 190);
		v = vectorsFromRect(corners);
		
		orthogonal(v[0], v[1]);
		orthogonal(v[1], v[2]);
		orthogonal(v[2], v[3]);
		orthogonal(v[3], v[0]);
		
		corners = Geometry.rotateRect(300, 4, 359);
		v = vectorsFromRect(corners);
		
		orthogonal(v[0], v[1]);
		orthogonal(v[1], v[2]);
		orthogonal(v[2], v[3]);
		orthogonal(v[3], v[0]);
	}
	
	@Test
	public void testGetPoint()
	{
		double deg = 60;
		double rad = Math.toRadians(deg);
		double sin = Math.sin(rad);
		double cos = Math.cos(rad);
		
		double xd = 5;
		double yd = 2;
		
		Point2D.Double p;
		
		// 1st quadrant
		p = Geometry.getPoint(deg, xd, yd);
		assertTrue(Math.abs(p.x - (xd * cos - sin * yd) ) < errorLimit);
		assertTrue(Math.abs(p.y - (yd * cos + sin * xd) ) < errorLimit);
		
		// 2nd quadrant
		p = Geometry.getPoint(deg + 60, xd, yd);
		assertTrue(Math.abs(p.x - (-xd * cos - sin * yd) ) < errorLimit);
		assertTrue(Math.abs(p.y - (-yd * cos + sin * xd) ) < errorLimit);
		
		// 3rd quadrant
		p = Geometry.getPoint(270 - 30, xd, yd);
		assertTrue(Math.abs(p.x - (-xd * cos + sin * yd) ) < errorLimit);
		assertTrue(Math.abs(p.y - (-yd * cos - sin * xd) ) < errorLimit);
		
		// 4th quadrant
		p = Geometry.getPoint(360 - deg, xd, yd);
		assertTrue(Math.abs(p.x - (xd * cos + sin * yd) ) < errorLimit);
		assertTrue(Math.abs(p.y - (yd * cos - sin * xd) ) < errorLimit);
	}
	
	@Test
	public void testGetPointWithOffset()
	{
		double deg = 60;
		double xd = 5;
		double yd = 2;
		
		Point2D.Double p = Geometry.getPoint(deg, xd, yd);
		
		double offsetx = 10;
		double offsety = -12;
		
		Point2D.Double p2 = Geometry.getPoint(deg, xd, yd, offsetx, offsety);
		
		assertTrue(Math.abs(p.x + offsetx - p2.x ) < errorLimit);
		assertTrue(Math.abs(p.y + offsety - p2.y ) < errorLimit);
	}
}
